#!/usr/bin/env python  

#Daniel Cook - 
#COMP4766 - A1

import roslib
roslib.load_manifest('comp4766_a1')
import rospy
# Add other import statements if necessary

# Import the formats needed for used services and messages
#from turtlesim.srv import ???
from turtlesim.msg import Color
from geometry_msgs.msg import Twist

def sensor_callback(color):
    currentColor = (color.r + color.g + color.b) / 3.0
    global lastColor, lastAngle

    # The turtle is controlled with a Twist message.  Execute the following
    # for more information on Twist: "rosmsg show geometry_msgs/Twist".  Just
    # as an example, the commands below create and publish a twist message with
    # a forwards speed of 1.0 and a turning rate of 0.1.
    if(currentColor > lastColor):
        twist = Twist()
        twist.linear.x = 1.0
        twist.angular.z = 0.0
        vel_publisher.publish(twist)
        lastAngle = 0.0
    elif(currentColor < lastColor):
        twist = Twist()
        twist.linear.x = 1.0
        twist.angular.z = 20.0
        vel_publisher.publish(twist)
        lastAngle = 20.0
    else:
        twist = Twist()
        twist.linear.x = 1.0
        twist.angular.z = lastAngle
        vel_publisher.publish(twist)        
        
    lastColor = currentColor

if __name__ == '__main__':
    global vel_publisher, color
    
    lastColor = 0.0
    lastAngle = 0.0

    rospy.init_node('homer')

    # Create a publisher so that we can output command velocities to the turtle.
    vel_publisher = rospy.Publisher("/turtle1/cmd_vel", Twist)

    # Subscribe to the color sensor.
    color = rospy.Subscriber("/turtle1/color_sensor", Color, sensor_callback)

    rospy.spin()
